﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace SuperMicroWave.Common
{
    public class PIDBase
    {
        public struct PidController
        {
            public float SetValue;                    //设定值
            public float Value;                 //当前值
            public float CurrentError;
            public float LastError;                    //倒数第一的错误
            public float PenultimateError;             //倒数第二的错误         
            public float Kp;
            public float Ti;
            public float Td;
            public float Output;
        }

        public PidController pidCtr = new PidController();

        public void PidInit()
        {
            pidCtr.SetValue = 0;
            pidCtr.Value = 0;
            pidCtr.CurrentError = 0;
            pidCtr.LastError = 0;
            pidCtr.PenultimateError = 0;
            pidCtr.Kp = 0;
            pidCtr.Ti = 0;
            pidCtr.Td = 0;
            pidCtr.Output = 0;
        }

        public void PIDSet(float p, float i, float d)
        {
            pidCtr.Kp = p;
            pidCtr.Ti = i;
            pidCtr.Td = d;
        }

        public float GetOutPutValue(float setValue, float value)
        {
            pidCtr.SetValue = setValue;
            pidCtr.Value = value;

            pidCtr.PenultimateError = pidCtr.LastError;
            pidCtr.LastError = pidCtr.CurrentError;
            pidCtr.CurrentError = pidCtr.SetValue - pidCtr.Value;

            //int index = 0;
            //if (pidCtr.CurrentError > 5 || pidCtr.CurrentError < -5)
            //    index = 0;
            //else
            //    index = 1;

            float UK = (pidCtr.Kp) * pidCtr.CurrentError +
                       (pidCtr.Ti) * (pidCtr.CurrentError - pidCtr.LastError) +
                       (pidCtr.Td) * ((pidCtr.CurrentError - pidCtr.LastError)
                       - (pidCtr.LastError - pidCtr.PenultimateError));
            if ((pidCtr.CurrentError - pidCtr.LastError) < 0)
            {
                UK += (float)((pidCtr.Ti) * (pidCtr.CurrentError - pidCtr.LastError) * 0.2);
            }
            pidCtr.Output += UK;

            if (pidCtr.Output >= 3500) pidCtr.Output = 3500;
            else if (pidCtr.Output <= 0) pidCtr.Output = 0;

            return pidCtr.Output;
        }
    }
}
